#include "rclcpp/rclcpp.hpp"
#include "sensor_msgs/msg/imu.hpp"
#include <iomanip>

/**
 * Version: 11 (Final Logic + Compile Fix)
 * This version correctly handles timestamps for both live and rosbag replay modes
 * and fixes the C++ syntax error for message types.
 */
class ImuFilterNode : public rclcpp::Node
{
public:
    ImuFilterNode() : Node("imu_filter_node"), 
                      last_raw_timestamp_(0, 0, RCL_ROS_TIME),
                      steady_clock_(RCL_STEADY_TIME)
    {
        // ROS 2的rclcpp会自动处理'use_sim_time'参数的声明，我们只需要获取它即可
        this->get_parameter("use_sim_time", using_sim_time_);

        if (using_sim_time_) {
            RCLCPP_FATAL(this->get_logger(), "IMU FILTER NODE V11 (FINAL) IS RUNNING IN **SIMULATION TIME** MODE!");
            RCLCPP_INFO(this->get_logger(), "In this mode, timestamps from the bag will be PRESERVED, not rewritten.");
        } else {
            RCLCPP_FATAL(this->get_logger(), "IMU FILTER NODE V11 (FINAL) IS RUNNING IN **WALL CLOCK** MODE!");
            RCLCPP_INFO(this->get_logger(), "In this mode, timestamps will be REWRITTEN with the steady clock.");
        }

        rclcpp::QoS qos_profile(100);
        qos_profile.reliability(RMW_QOS_POLICY_RELIABILITY_BEST_EFFORT);

        // 使用正确的C++消息类型语法: sensor_msgs::msg::Imu
        subscription_ = this->create_subscription<sensor_msgs::msg::Imu>(
            "/dog_imu_raw", 
            qos_profile,
            std::bind(&ImuFilterNode::imu_callback, this, std::placeholders::_1));
            
        publisher_ = this->create_publisher<sensor_msgs::msg::Imu>("/imu/data", qos_profile);
    }

private:
    // 使用正确的C++消息类型语法: sensor_msgs::msg::Imu
    void imu_callback(const sensor_msgs::msg::Imu::SharedPtr msg)
    {
        rclcpp::Time current_raw_timestamp = msg->header.stamp;

        if (current_raw_timestamp > last_raw_timestamp_ || last_raw_timestamp_.seconds() == 0)
        {
            auto corrected_msg = *msg;

            // 最终的核心逻辑
            if (using_sim_time_) {
                // 在仿真/回放模式下，保留来自rosbag的原始时间戳
            } else {
                // 只有在连接真实机器人时，才用物理时钟重写时间戳
                corrected_msg.header.stamp = steady_clock_.now();
            }
            
            publisher_->publish(corrected_msg);
            last_raw_timestamp_ = current_raw_timestamp;
        }
        else
        {
            RCLCPP_WARN_THROTTLE(
                this->get_logger(),
                *this->get_clock(),
                5000,
                "Original timestamp reversed! Dropping message.");
        }
    }
    
    // 使用正确的C++消息类型语法: sensor_msgs::msg::Imu
    rclcpp::Subscription<sensor_msgs::msg::Imu>::SharedPtr subscription_;
    rclcpp::Publisher<sensor_msgs::msg::Imu>::SharedPtr publisher_;
    rclcpp::Time last_raw_timestamp_;
    rclcpp::Clock steady_clock_;
    bool using_sim_time_;
};

int main(int argc, char * argv[])
{
    rclcpp::init(argc, argv);
    rclcpp::spin(std::make_shared<ImuFilterNode>());
    rclcpp::shutdown();
    return 0;
}